import networkx as nx
import pandas as pd
import matplotlib.pyplot as plt
from Bezier import bezier




# 读取路线数据
route_data = pd.read_csv('Type_Line.txt', delim_whitespace=True, header=None)
route_data.columns = ['start_lat', 'start_lon', 'end_lat', 'end_lon', 'channel_type']

# 创建有向图
G = nx.DiGraph()


import math

def calculate_distance(lat1, lon1, lat2, lon2):
    # 计算欧氏距离
    distance = math.sqrt((lat2 - lat1)**2 + (lon2 - lon1)**2)
    return distance
# 添加路线数据到有向图
for index, row in route_data.iterrows():
    if row['channel_type'] == 'Bothway':
        G.add_edge((row['start_lat'], row['start_lon']), (row['end_lat'], row['end_lon']),weight=calculate_distance(row['start_lat'], row['start_lon'], row['end_lat'], row['end_lon']))
        G.add_edge((row['end_lat'], row['end_lon']), (row['start_lat'], row['start_lon']),weight=calculate_distance(row['end_lat'], row['end_lon'], row['start_lat'], row['start_lon']))
    elif row['channel_type'] == 'Forward':
        G.add_edge((row['start_lat'], row['start_lon']), (row['end_lat'], row['end_lon']),weight=calculate_distance(row['start_lat'], row['start_lon'], row['end_lat'], row['end_lon']))


# 读取站台数据
station_data = pd.read_csv('agv_station_gai.txt', delim_whitespace=True, header=None)
station_data.columns = ['lat', 'lon', 'type', 'number']


# print(station_data.loc[0,'lat'], station_data.loc[0,'lon'])
# print(station_data.loc[10,'lat'], station_data.loc[10,'lon'])


# 添加站台数据到有向图
for index, row in station_data.iterrows():
    G.add_node((row['lat'], row['lon']), type=row['type'], number=row['number'])
px,py,points,bezier_data = bezier()
# [[p3_lat, p3_lon], [p1_lat, p1_lon], [p2_lat, p2_lon], [p4_lat, p4_lon]]
# [[p4_lat, p4_lon],[p2_lat, p2_lon], [p1_lat,p1_lon], [p3_lat, p3_lon]]

for index, row in bezier_data.iterrows():
    if row['channel_type'] == 'Bothway':
        G.add_edge((row['p3_lat'], row['p3_lon']), (row['p4_lat'], row['p4_lon']),weight=calculate_distance(row['p3_lat'], row['p3_lon'], row['p4_lat'], row['p4_lon']))
        G.add_edge((row['p4_lat'], row['p4_lon']), (row['p3_lat'], row['p3_lon']),weight=calculate_distance(row['p4_lat'], row['p4_lon'], row['p3_lat'], row['p3_lon']))
    elif row['channel_type'] == 'Forward':
        G.add_edge((row['p3_lat'], row['p3_lon']), (row['p4_lat'], row['p4_lon']),weight=calculate_distance(row['p3_lat'], row['p3_lon'], row['p4_lat'], row['p4_lon']))

if __name__ == '__main__':
    # 可视化地图
    pos = {node: (node[0], node[1]) for node in G.nodes()}
    plt.figure(figsize=(10, 10))
    plt.scatter(19.558655,16.435793,color='black')

    plt.scatter(18.169516,  28.700434,color='black')
    nx.draw(G, pos, node_size=5, node_color='lightblue', font_size=8, arrowstyle='-', arrowsize=5)
    plt.plot(px,py,color='green')

    plt.show()

